﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using haptic.src.engine.debug;

namespace haptic.src.engine.collisions
{
    [Serializable]
    public class AABBCollider : ICollider
    {
        #region Constructor
        /// <summary>
        /// only for serialization
        /// </summary> 
        public AABBCollider()
        {
        }
        public AABBCollider(Vector2 vCenter, Vector2 vRadius)
        {
            Set(vCenter, vRadius);
        }
        public AABBCollider(Vector2 vCenter, Vector2 vSize, Vector2 vOldCenter)
        {
            Set(vCenter, vSize, vOldCenter);
        }
        public AABBCollider(Vector2 vCenter, Vector2 vSize, Vector2 vOldCenter, Vector2 vOldSize)
        {
            Set(vCenter, vSize, vOldCenter, vOldSize);
        }
        public AABBCollider(AABBCollider oC)
        {
            m_vCenter = oC.m_vCenter;
            m_vExtents = oC.m_vExtents;
            m_vOldExtents = oC.m_vOldExtents;
            m_vOldCenter = oC.m_vOldCenter;
        }
        public ICollider Clone()
        {
            return new AABBCollider(this);
        }

        #endregion
        #region Edit
        public void Set(Vector2 vCenter, Vector2 vRadius)
        {
            m_vCenter = vCenter;
            m_vExtents = vRadius;
            m_vOldCenter = vCenter;
            m_vOldExtents = m_vExtents;
        }
        public void Set(Vector2 vCenter, Vector2 vRadius, Vector2 vOldCenter)
        {
            m_vCenter = vCenter;
            m_vExtents = vRadius;
            m_vOldCenter = vOldCenter;
            m_vOldExtents = m_vExtents;
        }
        public void Set(Vector2 vCenter, Vector2 vRadius, Vector2 vOldCenter, Vector2 vOldRadius)
        {
            m_vCenter = vCenter;
            m_vExtents = vRadius;
            m_vOldCenter = vOldCenter;
            m_vOldExtents = vOldRadius;
        }
        public void SetCenter(Vector2 vCenter)
        {
            m_vCenter = vCenter;
        }
        #endregion
        #region Collisions
        public bool IsInside(Vector2 vPos)
        {
            Vector2 vRelPos = vPos - m_vCenter;
            return (vRelPos.X >= 0.0f && vRelPos.Y >= 0.0f && vRelPos.X <= m_vExtents.X && vRelPos.Y <= m_vExtents.Y);
        }
        public bool IsInside(ICollider oCollider)
        {
            if (oCollider.GetType() == typeof(AABBCollider) )
            {
                AABBCollider oC = (AABBCollider)oCollider;
                return IsInside(oC.m_vCenter) && IsInside(oC.m_vCenter + oC.m_vExtents);
            }
            return false;
        }

        public bool IsColliding(AABBCollider oC, CollisionHit oCollisionHit)
        {
            Debug.Assert(oC != null);
            if (Math.Abs(m_vCenter.X - oC.m_vCenter.X) > (m_vExtents.X + oC.m_vExtents.Y))
                return false;
            if (Math.Abs(m_vCenter.Y - oC.m_vCenter.Y) > (m_vExtents.Y + oC.m_vExtents.Y))
                return false;
            if (oCollisionHit != null)
            {
                // Target contact
                Vector2 vContact = m_vCenter;
                Vector2 vMin = oC.m_vCenter - oC.m_vExtents;
                Vector2 vMax = oC.m_vCenter + oC.m_vExtents;
                if (vContact.X < vMin.X) vContact.X = vMin.X;
                else if (vContact.X > vMax.X) vContact.X = vMax.X;
                if (vContact.Y < vMin.Y) vContact.Y = vMin.Y;
                else if (vContact.Y > vMax.Y) vContact.Y = vMax.Y;
                oCollisionHit.m_vTargetContact = vContact;

                // Normal
                oCollisionHit.m_vNormal = m_vCenter - oCollisionHit.m_vTargetContact;
                if (Math.Abs(oCollisionHit.m_vNormal.X) > Math.Abs(oCollisionHit.m_vNormal.Y))
                    oCollisionHit.m_vNormal.Y = 0.0f;
                else
                    oCollisionHit.m_vNormal.X = 0.0f;
                if (oCollisionHit.m_vNormal != null)
                    oCollisionHit.m_vNormal.Normalize();

                // Source contact
                vContact = oC.m_vCenter;
                vMin = m_vCenter - m_vExtents;
                vMax = m_vCenter + m_vExtents;
                if (vContact.X < vMin.X) vContact.X = vMin.X;
                else if (vContact.X > vMax.X) vContact.X = vMax.X;
                if (vContact.Y < vMin.Y) vContact.Y = vMin.Y;
                else if (vContact.Y > vMax.Y) vContact.Y = vMax.Y;
                Vector2 vSeparatingVector = Vector2.Dot(vContact - oCollisionHit.m_vTargetContact, oCollisionHit.m_vNormal) * oCollisionHit.m_vNormal;
                oCollisionHit.m_vSourceContact = oCollisionHit.m_vTargetContact + vSeparatingVector;
            }
            return true;
        }

        public bool IsColliding(BorderCollider oC, CollisionHit oCollisionHit)
        {
            Debug.Assert(oC != null);
            Vector2 vTmp = new Vector2(m_vExtents.X, -m_vExtents.Y);
            Vector2[] vP = new Vector2[4];
            vP[0] = m_vCenter - m_vExtents;
            vP[1] = m_vCenter + vTmp;
            vP[2] = m_vCenter + m_vExtents;
            vP[3] = m_vCenter - vTmp;

            float[] fD = new float[4];

            fD[0] = Vector2.Dot(oC.vNormal, vP[0]);
            fD[1] = Vector2.Dot(oC.vNormal, vP[1]);
            fD[2] = Vector2.Dot(oC.vNormal, vP[2]);
            fD[3] = Vector2.Dot(oC.vNormal, vP[3]);

            if (fD[0] > oC.fC && fD[1] > oC.fC && fD[2] > oC.fC && fD[3] > oC.fC)
                return false;

            if (oCollisionHit != null)
            {
                oCollisionHit.m_vNormal = oC.vNormal;

                int iMin = 0;
                for (int i = 1; i < 4; i++)
                {
                    if (fD[i] < fD[iMin])
                        iMin = i;
                }
                Vector2 vPMin = vP[iMin];
                float fDMin = fD[iMin];

                oCollisionHit.m_vTargetContact = vPMin - (fDMin - oC.fC) * oC.vNormal;
                oCollisionHit.m_vSourceContact = vPMin;
            }
            return true;
        }

        public bool IsColliding(ICollider oCollider, CollisionHit oCollisionHit)
        {
            if (oCollider == null)
                return false;
            if (oCollider.GetType() == typeof(AABBCollider))
                return IsColliding((AABBCollider)oCollider, oCollisionHit);
            else if (oCollider.GetType() == typeof(BorderCollider))
                return IsColliding((BorderCollider)oCollider, oCollisionHit);
            return false;
        }

        public bool IsColliding(ICollider oCollider)
        {
            return IsColliding(oCollider, null);
        }

        #endregion

        public BoundingBox GetBoundingBox()
        {
            return new BoundingBox(m_vCenter, m_vExtents);
        }

        #region Draw
        public void DrawDebug(DebugRenderer oDebug)
        {
            oDebug.DrawBox(m_vCenter - m_vExtents, m_vExtents + m_vExtents, Color.Red);
        }
        #endregion

        #region Attributes
        private Vector2 m_vCenter;
        public Vector2 vCenter { get { return m_vCenter; } }
        private Vector2 m_vOldCenter;
        private Vector2 m_vExtents;
        public Vector2 vExtents { get { return m_vExtents; } }
        private Vector2 m_vOldExtents;
        #endregion
    }
}
